ROS2 gazebo camera sensor and plugin
Last update: 06-01-23
Table of Content
Gazebo camera sensor with ROS2 plugin Tutorial Using SDF,
- Spawn camera SDF model into gazebo
- View Camera with correct TF in rviz
- set plugin camera coordinate system (x:right, y:down, z:into the plan)
Image Coordinate Frame#
Camera Coordinate Frame#
Robot Coordinate Frmae#
Demo#
- camera.world: Gazebo world sdf file
- camera2.sdf: camera model, SDF file with ros2 plugin
- camera.launch.py: ROS2 launch file
- Launch gazebo
- Run Rviz
- spawn ROBOT (camera)
- Set static TF’s
world#
Basic gazebo world - Add simple object viewed by the camera
worlds/camera.world
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<pose>2 0 0 0 0 0</pose>
<uri>model://construction_cone</uri>
</include>
</world>
</sdf>
model#
models/camera2/model.sdf
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="camera2">
<link name="camera_link">
<frame name="camera_optical" attached_to="camera_link">
<pose relative_to="camera_link">0.05 0 0 -1.575 0 -1.575</pose>
</frame>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="camera" type="camera">
<pose>0.05 0 0 0 0 0</pose>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<plugin name="camera" filename="libgazebo_ros_camera.so">
<!-- Change namespace, camera name and topics so -
* Images are published to: /custom_ns/custom_camera/custom_image
* Camera info is published to: /custom_ns/custom_camera/custom_info
-->
<!-- <ros>
<namespace>custom_ns</namespace>
<remapping>image_raw:=custom_img</remapping>
<remapping>camera_info:=custom_info</remapping>
</ros> -->
<!-- Set camera name. If empty, defaults to sensor name (i.e. "sensor_name") -->
<!-- <camera_name>custom_camera</camera_name> -->
<!-- Set TF frame name. If empty, defaults to link name (i.e. "link_name") -->
<frame_name>camera_optical</frame_name>
</plugin>
</sensor>
</link>
</model>
</sdf>
launch#
launch/camera.launch.py
from launch import LaunchDescription
import os
from math import pi
from ament_index_python.packages import get_package_share_directory
from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
import xacro
PACKAGE = "ros2_gazebo_tutorial"
WORLD = "camera.world"
MODEL="camera2"
SDF="model.sdf"
def generate_launch_description():
ld = LaunchDescription()
pkg = get_package_share_directory(PACKAGE)
gazebo_pkg = get_package_share_directory('gazebo_ros')
verbose = LaunchConfiguration("verbose")
arg_gazebo_verbose = DeclareLaunchArgument("verbose", default_value="true")
world = LaunchConfiguration("world")
arg_gazebo_world = DeclareLaunchArgument("world", default_value=WORLD)
sim_time = LaunchConfiguration("sim_time")
arg_sim_time = DeclareLaunchArgument("sim_time", default_value="true")
resources = [os.path.join(pkg, "worlds")]
resource_env = AppendEnvironmentVariable(
name="GAZEBO_RESOURCE_PATH", value=":".join(resources)
)
models = [os.path.join(pkg, "models")]
models_env = AppendEnvironmentVariable(
name="GAZEBO_MODEL_PATH", value=":".join(models)
)
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
gazebo_pkg, 'launch', 'gazebo.launch.py')]),
launch_arguments={'verbose': verbose, "world": world}.items()
)
robot_description_path = os.path.join(pkg, "models", MODEL, SDF)
doc = xacro.parse(open(robot_description_path))
xacro.process_doc(doc)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[
{
'use_sim_time': sim_time,
'robot_description': doc.toxml()
}
]
)
spawn_entity = Node(
package="gazebo_ros",
executable="spawn_entity.py",
arguments=["-entity", "demo", "-topic", "robot_description", "-z", "0.5"],
output="screen",
)
tf = Node(
package="tf2_ros",
executable="static_transform_publisher",
output="screen",
arguments=["0","0","0","0","0","0","world","camera_link"]
)
tf_camera_optical = Node(
package="tf2_ros",
executable="static_transform_publisher",
output="screen",
arguments=["0.05","0","0",str(-pi/2),"0",str(-pi/2),"camera_link","camera_optical"]
)
rviz_node = Node(
package='rviz2',
namespace='',
executable='rviz2',
name='rviz2',
arguments=['-d' + os.path.join(pkg, 'config', 'camera.rviz')]
)
ld.add_action(models_env)
ld.add_action(resource_env)
ld.add_action(arg_gazebo_verbose)
ld.add_action(arg_gazebo_world)
ld.add_action(arg_sim_time)
ld.add_action(gazebo)
ld.add_action(robot_state_publisher)
ld.add_action(spawn_entity)
ld.add_action(tf)
ld.add_action(tf_camera_optical)
ld.add_action(rviz_node)
return ld